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Report  Documentation  Page 

On-Line  Trajectory  Optimization 
for  Autonomous  Air  Vehicles 

Technical  Abstract 

Successful  operation  of  next-generation  unmanned  air  vehicles  will  demand  a  high  level 
of  autonomy.  Autonomous  low-level  operation  in  a  hi^-threat  environment  dictates  a  need  for 
on-board,  robust,  reliable  and  efficient  trajectory  optimization.  In  this  report,  we  develop  and 
demonstrate  an  innovative  combination  of  traditional  analytical  and  numerical  solution 
procedures  to  produce  efficient,  robust  and  reliable  means  for  nonlinear  flight  path  optimization 
in  the  presence  of  time-varying  obstacles  and  threats.  The  solution  procedure  exploits  the  natural 
time-scale  separation  that  exists  in  the  aircraft  dynamics  using  singular  perturbation  theory.  A 
reduced  order  problem  involving  only  the  kinematics  of  the  position  subspace  is  treated 
numerically.  The  nonlinear  aircraft  dynamics  are  to  be  treated  analytically  in  phase  11  using  a 
boundary  layer  analysis  that  results  in  an  optimal  feedback  guidance  solution.  The  developed 
algorithms  were  coupled  with  a  neural  network  adaptive  autopilot  and  integrated  in  an  existing 
unmanned  testbed.  This  report  documents  the  phase  I  effort,  which  produced  a  demonstration  of 
the  developed  algorithm  in  near-real-time  flight  simulation,  and  included  a  simple  evaluation  of 
tracking  computed  trajectories  on  a  rotary  wing  UAV.  The  envisioned  Phase  n  program  will 
expand  the  scope  of  mission  scenarios  that  can  be  addressed,  will  produce  flight  code  validated 
in  real-time  hardware-in-the-loop  simulation,  and  culminate  in  close  range  flight  demonstration 
of  the  developed  technology  on  a  fixed-wing  unmanned  aircraft  testbed. 
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1.  Introduction 


1.1  Problem/Opportunity 

High-flying  unmanned  reconnaissance  and  surveillance  systems  are  now  being  used 
extensively  in  the  United  States  military.  Current  development  programs  will  soon  produce 
demonstrations  of  next-generation  unmanned  flight  systems  that  are  designed  to  perform  combat 
missions.  In  practice,  these  vehicles  must  achieve  a  high  level  of  autonomy  in  operations  to  be 
successfully  deployed  in  large  numbers.  Their  use  in  first-strike  combat  operations  will  dictate 
operations  in  densely  cluttered  environments  that  include  unknown  obstacles  and  threats,  and 
will  require  the  use  of  terrain  for  masking.  The  demand  for  autonomy  of  operations  in  such 
environments  dictates  the  need  for  an  on-board  trajectory  optimization  capability. 

1.2  Overall  Program  Objective 

The  original  phase  I  program  solicitation  establishes  the  overall  program  objective  as: 

“Develop,  implement,  and  demonstrate  algorithms  for  real-time  trajectory 
generation  of  autonomous  nonlinear  flight  systems.  The  algorithms  must  be  capable  of 
dealing  with  complex  nonlinear  vehicle  dynamics,  actuator  and  system  constraints,  as 
well  as  fixed  and  pop-up  threats 

1.3  Background  on  Traditional  Methods 

In  any  given  mission  scenario,  the  initial  conditions,  requirements,  objectives,  vehicle 
dynamics,  and  constraints  can  be  analyzed  at  varying  levels  of  detail  to  produce  a  number  of 
feasible  flight  plans.  However,  in  most  situations  conflicts  will  arise  between  the  cited  factors 
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that  necessitate  trade-off  of  one  for  the  other.  Ad  hoc  methods  of  solution  can  be  constructed  for 
simple  missions  with  relatively  few  constraints,  but  such  methods  are  quickly  overwhelmed  as 
problem  complexity  grows.  In  such  cases  formal  methods  for  trajectory  optimization  are  needed 
to  identify  a  dynamically  feasible  solution  that  is  “best”  in  some  sense. 

Methods  available  for  solution  of  trajectory  optimization  problems  generally  fall  into  two 
distinct  categories:  direct  and  indirect.  The  former,  which  are  numerical,  seek  to  minimize  or 
maximize  the  performance  index  directly  by  judicious  choice  of  free  parameters.  The  latter,  for 
which  analytic  solutions  can  sometimes  be  derived,  at  least  for  problems  of  low  order,  seek  to 
minimize  or  maximize  the  performance  index  indirectly  by  satisfying  the  first  order  necessary 
conditions  for  optimality  that  result  from  the  application  of  the  Calculus  of  Variations. 

The  direct  approach  first  requires  parameterization  of  the  controls  using  discretization, 
spline  fit,  or  the  like.  A  parameter  optimization  algorithm  is  then  employed  to  sequentially 
improve  an  initial  guess  of  the  state  and  control  histories.  Many  such  algorithms  are  in  general 
use  today  and  include  variations  on  gradient  and  variable  metric  techniques. 

The  application  of  the  necessary  conditions  for  optimality  (the  indirect  approach) 
generally  results  in  a  two-point  boimdary  value  problem  (TPBVP).  Analytic  solutions  are 
generally  unobtainable  except  for  problems  of  low  order,  and  thus  one  must  usually  resort  to 
numerical  methods  of  solution.  Nonlinear  TPBVP  are  generally  attacked  using  initial  value  or 
finite-difference  methods.  Initial  value  methods,  often  referred  to  as  shooting  methods. 


2 


continually  satisfy  the  given  initial  conditions  and  the  differential  equations.  A  nominal  solution 
is  generated  by  guessing  the  missing  initial  conditions  and  integrating  the  differential  equations 
forward  in  time.  Bisection  or  other  root-finding  algorithms  are  then  employed  to  reduce  the 
cumulative  error  in  the  final  conditions  with  each  iteration.  In  contrast,  finite-difference 
methods,  also  referred  to  as  quasi-linearization  methods,  generally  satisfy  the  initial  and  final 
conditions  at  each  stage  of  the  process  while  violating  the  differential  equations  to  some  degree. 
In  between  the  pure  shooting  and  finite  difference  methods  fall  the  multi-point  methods. 

While  great  success  has  been  achieved  in  numerically  solving  complex  nonlinear 
trajectory  optimization  problems  using  direct  and  indirect  techniques,  many  short  comings  still 
exist.  Parameter  optimization  schemes  tend  to  be  tolerant  of  a  poor  initial  guess  but  are  in 
general  slow  to  converge.  Indirect  methods,  in  contrast,  can  usually  achieve  greater  accuracy  in 
fewer  iterations  but  also  exhibit  much  greater  sensitivity  to  the  initial  guess.  Essentially  all  of  the 
available  methods  in  both  categories  demand  computational  resources  that  until  recently  were 
simply  unavailable  in  a  flight  environment.  For  complex  problem  formulations  with  full  vehicle 
dynamics,  most  have  run  times  that  preclude  real-time  mission  updates.  Almost  all  suffer  from 
great  complexity  of  implementation. 

For  these  reasons,  past  aircraft  trajectory  generation  studies  were  rooted  in  classical 
optimization  theory  and  the  calculus  of  variations.  The  focus  was  on  up  and  away  flight,  with 
minimal  concern  for  nap  of  the  Earth  maneuvering  flight.  A  great  variety  of  practical  methods 
for  optimizing  the  flight  of  aircraft  both  in  two  and  three  dimensions  were  developed  in  the 
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1960s  and  70s.  Much  of  this  work  began  with  the  studies  performed  by  Bryson  based  on 
minimizing  time,  fuel  or  range  using  reduced  order  modeling  methods  [1-3].  Subsequently,  the 
use  of  singular  perturbation  methods,  and  multi-time  scale  analysis  for  trajectory  optimization 
were  suggested  by  Kelley  [4,5],  and  later  extensively  developed  and  applied  by  Calise  [6-9], 
Ardema  [10]  and  others.  A  recent  survey  article  may  be  found  in  [11].  In  the  work  of  Calise, 
the  emphasis  was  on  obtaining  analytic  or  near-analytic  feedback  solutions  that  could  be 
implemented  with  very  limited  computing  resources  in  real-time. 

Singularly  perturbed  dynamic  systems  are  characterized  by  a  small  parameter  multiplying 
the  derivatives  of  some  state  vector  components.  If  the  parameter  epsilon  is  set  to  zero,  the  order 
of  the  dynamic  system  is  reduced.  The  solution  to  the  reduced  order  problem  that  results  when 
epsilon  is  set  to  zero  provides  the  leading  term  in  a  perturbation  series.  Of  course,  the  reduced 
order  solution  is  not  able  to  satisfy  all  the  boundary  conditions  of  the  original  full  order  problem. 
This  is  compensated  for  by  constructing  bormdary  layer  transients  that  allow  rapid  variation  of 
the  fast  states  on  a  stretched  time  scale.  The  boundary  layer  solutions  are  required  to  satisfy  the 
boundary  conditions  violated  by  the  reduced  solution  and  to  approach  the  reduced  solution 
asymptotically. 

Natural  time  scale  separation  that  exists  in  aircraft  dynamics  allows  the  successful 
application  of  singular  perturbation  methods  to  problems  in  atmospheric  flight  path  optimization 
[12].  The  reduced  and  boundary  layer  problems  are  often  solvable  using  analytic  methods  alone. 
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This  has  made  a  unified  analysis  of  3-D  high-performance  aircraft  path  optimization  possible 
[13]. 


1.4  Proposed  Innovation 

If  we  now  consider  flight  of  high  performance  aircraft  at  low  levels  where  terrain  and 
obstacles  must  be  accounted  for,  the  reduced  order  problem  can  no  longer  be  solved  in  a  purely 
analytic  fashion.  In  this  work,  we  exploit  the  natural  time-scale  separation  in  the  flight  vehicle 
dynamics,  and  the  method  of  matched  asymptotic  expansions  described  above  to  analytically 
treat  the  nonlinear  aircraft  dynamics  and  associated  constraints,  and  to  address  the  path  planning 
portion  of  the  problem  using  numerical  methods  applied  to  only  the  kinematics  in  the  x,  y 
subspace.  That  is,  numerical  methods  are  used  to  solve  the  reduced  order  problem,  and  analytic 
solutions  will  be  used  to  construct  the  boundary  layer  solutions.  This  novel  approach  builds  on 
decades  of  previous  work  in  efficient  flight  path  optimization  and  real-time  optimal  guidance, 
and  focuses  the  use  of  numerical  solution  procedures  on  only  that  portion  of  the  problem  for 
which  fully  analytic  solutions  are  not  available.  The  result  is  robust,  autonomous,  real-time 
optimal  trajectory  generation  for  nonlinear  flight  systems  in  complex  mission  scenarios. 
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2.  Reduced  Order  Problem 


In  the  early  1990s,  P.  K.  Menon  and  Eulgon  Kim  researched  methods  of  optimal  trajectory 
path  planning  for  terrain  following  and  terrain  masking  flight.  This  research  produced  a  reduced 
order  formulation  based  on  a  constant  velocity  approach  [14,15].  This  chapter  begins  by 
expanding  on  the  work  done  by  Menon  and  Kim  while  using  a  constant  energy  approach. 

2.1  A  Simplified  Formulation 

Consider  the  equations  of  motion  for  a  flight  vehicle  written  in  a  local  level  frame. 


• 

X  =  Vcos\j/ 

(1) 

• 

y  =  Vsm\if 

(2) 

where  x  and  y  are  the  position  coordinates,  vi;  is  the  heading  angle  (and  also  the  control  variable) 
and  V  is  the  horizontal  component  of  velocity.  If  the  vertical  component  of  velocity  is  small, 
then  V  can  approximately  related  to  energy  per  imity  mass  (E)  by 

V  =  pg(E-Mx,y)-K)  (3) 

In  this  expression  he  is  the  groimd  clearance  and  f\(x,y)  denotes  the  terrain  altitude  profile. 
Note  that  (3)  embodies  the  constraint  that  the  vehicle  maintains  a  fixed  height  above  the  terrain. 
However,  this  is  only  approximately  the  case  since  V  in  this  formulation  is  not  the  total  velocity. 
The  cost  equation  is 


6 


J=^^[(\-K)  +  Kf(x,yj\dt 


(4) 


The  function  f(x,y)  denotes  a  combined  penalty  function  of  the  terrain  and  threats.  The 
weighting  parameter,  K,  can  vary  between  0  and  1  and  determines  the  amount  of  terrain  masking 
and  threat  avoidance  used  in  the  optimization.  When  K=0,  the  equations  are  optimized  with 
respect  to  time.  When  K  is  set  to  1,  the  path  is  optimized  with  respect  to  the  threats  and  the 
terrain.  For  example,  if  f{x,y)  =  /i(x,y),  then  the  path  that  minimizes  the  integrated  altitude 
profile  is  foimd.  More  generally  a  weighted  function  of  the  terrain  and  threats  will  be  employed, 
f(x,y)  =  fi(x,y)  +  f2(x.y),  where  f2(x,y)  is  used  to  introduce  a  penalty  for  flying  in  the 
vicinity  of  a  threat.  Values  of  AT  between  0  and  1  correspond  to  a  compromise  solution. 

The  Hamiltonian  equation  is  represented  as 

H  =  l-K  +  Kf(x,y)  +  X^Vcosy/  +  A,yVsiny/  (5) 

Here,  and  are  the  costates.  The  differential  equations  for  the  costates  are: 


dH 

dx 


— 


dH 

8y 


Evaluating  these  derivatives  results  in  the  expressions 

A,x  =  -Kf^  +  cost//  +  Ay  sin^;^] 

Ay  =  -Kfy  +  [4  cosy/  +  Ay  sin^] 


(6) 

(7) 

(8) 

(9) 
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In  this  expression,  fxJu  and  fy.f\y  denote  the  partial  derivatives  of  f(x.y),fi(x.y)  with 
respect  to  x  and  y,  respectively.  The  optimality  condition  is 

—  =  0  (10) 

dy/ 

which  results  in 

(11) 

In  this  formulation,  the  final  time  is  free,  and  since  the  formulation  is  time  invariant,  i/  =  0 
all  along  the  trajectory.  Using  this  fact  together  with  (1 1),  it  is  possible  to  obtain  the  following 
solutions  for  the  costate  variables 


,  -{\.-K  +  Kf)cosii/ 

—  - - 

"  V 

-{\-K  +  Kf)smxir 


Taking  the  time  derivative  of  (12)  results  in 


(12) 

(13) 


-  Kf^  x+KL  y  cosv^  /,^  x+f^^  y  \(\-K+Kf)cos,\j/ 


{l-K+Kfyyrsmy/ 


(14) 


This  expression  can  be  used  in  (8)  to  solve  for  the  heading  angle  rate 
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where 


A,  =  Kf^V^  +  f,,A,g 

A,  =  +  A^A,g 

A,=l-K  +  Kf 


(16) 


(17) 

(18) 


At  this  point,  the  problem  to  be  solved  has  been  reduced  to  three  state  equations  (1),  (2)  and  (15) 
to  be  solved,  with  only  one  unknown  parameter  to  be  determined,  ^i(O),  the  initial  heading 
angle.  Thus  the  family  of  extremals  is  parameterized  by  \\i( 0) . 


2.2  The  Local  Tangent  Plane  Formulation 


Consider  next  a  formulation  that  incorporates  the  constraint  that  the  vehicle  flies 
tangentially  to  the  local  terrain  directly  into  the  equations  of  motion.  This  is  done  by  using  the 
formulation  in  References  14  and  15  that  starts  out  by  writing  the  equations  of  motion  in  a  local 
tangent  frame.  In  the  local  tangent  frame,  the  form  of  these  equations  become  identical  in  form 
to  (1)  and  (2),  however  V  represents  the  total  velocity  in  this  formulation.  Consequently  the 
approximation  embodied  in  the  first  formulation  presented  above  is  eliminated.  After 
transformation  to  the  local  level  frame,  the  equations  motion  become: 


•  Vcosw  Vfjsinxi/ 

X= - —■>r - - - 

*  -VAySinw 


(19) 

(20) 


where. 


V  =  pg(E-f(x,y)-h^)  (21) 

A=^|^  (22) 

(23) 
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The  control  variable  is  still  v|/,  the  local  heading  angle,  and  the  cost  equation  is  the  same  as 
stated  in  (4). 

The  Hamiltonian  for  this  system  becomes 


A  AA  I 


■FA^  sin^ 


and  the  costate  equations  are  found  to  be: 

k  =-KX +C;[q  +C4  sirf  y/-Q  coi  ^^/■+Q(cos^^Xsiny^)]  (25) 

k  =  -KX  +C,  [q  +Q  sirf  coi  y/+Q  (cos^^/Xsin^^)]  (26) 

where, 


(27) 

■®2  =  fxxfx  +  fyfxy 

(28) 

B3  =fyyfy+fxfxy 

(29) 

B4  =  fyyfx  +  fyfxy 

(30) 

C  = 

'  A^AlV^ 

(31) 

C,=-gf,AUl 

(32) 
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Cj  = 

(33) 

(34) 

(35) 

c,=-gr,A^Ai 

(36) 

C,  =  AiV^fJ„ 

(37) 

c,=c,-a;‘v’b, 

(38) 

A,viA’B.-2/‘/,/J 

(39) 

The  corresponding  optimality  condition  is 


tan^  = 


Following  the  same  procedure  as  described  for  the  simplified  formulation 
(40)  can  be  used  to  solve  for  and  Xy : 


(40) 

above,  (24)  and 


^  -A^A^cosxj/ 

^  V 

^  AXA^s\r\\j/-fJ^cos\i/) 

- 

Taking  the  time  derivative  of  (41)  and  setting  it  equal  to  (25)  yields  the  following  expression  for 
the  heading  angle  rate: 


(41) 

(42) 
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where: 


•  D,  sin^ +  Z),  cosu/" 
w  =  — - - - - 

D, 

(43) 

D,^A,V'f,E,-AfA,E, 

(44) 

^f;f,E,-A;vf,E, 

(45) 

D, = a;a,a,v‘ 

(46) 

E,=A;r‘K-A,V‘f^*gA;A,V 

(47) 

E,=A^K  +  gAlA,-A,V^f„ 

(48) 

e,  =  aj„-a]k 

(49) 

E.=gA,Vf.*^ 

A 

(50) 

As  with  the  previous  formulation,  the  system  is  now  reduced  to  solving  three  differential 
equations  with  one  unknown  parameter,  the  initial  heading. 
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2,3  Introducing  a  Heading  Rate  Constraint 


A  constraint  on  the  heading  rate  can  be  incorporated  by  introducing  the  equation 

y/  =  u  (51) 

where  u  is  the  new  control  variable.  The  heading  rate  constraint  is  modeled  by  imposing  the 
following  constraint  on  u 

\u\<c  (52) 

The  constraint  is  adjointed  to  the  Hamiltonian  by  using  constraint  multipliers.  This  results  in 

•  • 

H  =  \-K  +  Kf  +  +  ]H^(u-c)  +  n^i-u-c)  (53) 

In  this  expression,  is  the  costate  variable  corresponding  to  the  newly  introduced  \\i  state,  and 
jiii  and  H2  and  are  multipliers  used  to  impose  the  upper  and  lower  limits  on  the  heading  rate. 
When  the  control  is  not  on  one  of  the  constraints,  /ii  and  H2  are  both  equal  to  zero.  When  the 
control  is  on  one  of  the  constraints,  the  corresponding  multiplier  is  positive. 

When  not  on  the  constraint  we  have  a  singular  solution 

^  =  0  =  (54) 

ou  ^ 

which  further  implies  that  1^=0.  The  differential  equations  for  the  costates  are  found  from  the 
following  relations: 
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dH 

(55) 

dx 

dH 

(56) 

dy 

II 

o 

(57) 

dy/ 

Equations  (55)  and  (56)  are  the  same  as  those  obtained  earlier  in  (8)  and  (9)  or  (25)  and  (26),  and 
(57)  reduces  to  the  optimality  conditions  in  (11)  or  (40).  Therefore,  the  portion  of  the  trajectory 
in  which  the  constraint  is  not  on  the  limit  may  be  solved  the  same  way  as  described  earlier  for  the 
unconstrained  case. 

When  the  optimality  conditions  result  in  a  solution  that  violates  the  upper  limit,  =  0,  while 
//;  >  0  and  the  new  optimality  equation  is 

^  =  0  =  1, +M  (58) 

ou 

which  results  in  the  relationship 

(59) 

The  Hamiltonian  can  then  be  written  as 

H  =  \-K  +  Kf  +  X^x+x/y+^.^c  (60) 

The  differential  equations  for  Xx  and  Xy  remain  unchanged  while  the  differential  equation  for  Xy, 
is  found  to  be 

Xy,  -  XJ^ siny/ - XyV cosy/  (61) 


or 
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(62) 


M 

for  the  first  and  second  formulations,  respectively.  This  results  in  a  system  of  six  differential 
equations  that  must  be  solved  for  along  a  constrained  arc.  When  the  problem  solution  does  not 
begin  on  a  constraint,  there  are  no  unknown  parameters  to  be  found  when  a  constraint  is  hit.  All 
of  the  initial  conditions  are  equal  to  their  respective  values  at  the  point  of  entering  the  constraint, 
which  can  be  easily  calculated.  However,  if  the  solution  begins  on  the  constraint,  the  initial 
conditions  for  the  three  costate  differential  equations  —  equations  (55),  (56)  and  (57)  —  must  be 
solved  for. 

The  constrained  solution  for  the  case  where  the  unconstrained  solution  violates  the  lower 
limit  mirrors  the  constrained  solution  obtained  for  the  case  where  it  violates  the  upper  limit.  The 
only  difference  is  in  the  optimality  condition. 

^  =  0  =  A,-,.,  (63) 

ou 

which  yields  the  relationship 

=  7^2  (64) 

Here  the  new  Hamiltonian  can  be  written  as 

H  =  1  —  AT  +  Kf  +  x+  Xy  y—  X^c  (b5) 


2.4  Formulating  THE  Alpha  Limit 

Another  possible  method  of  constraining  the  turn  rate  is  through  the  analysis  of  the  alpha  —  or 
angle-of-attack  —  limit.  The  equations  for  heading  and  flight  path  angle  rate  are: 
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mVxj/  =  Lsin  n  /  cosy 
mVy  =  Lcos  fi-W  cosy 


(66) 


where  V  is  the  total  velocity,  ^  is  the  heading,  y  is  the  flight  path  angle,  L  is  the  lift,  /j.  is  the 
bank  angle,  and  m  is  the  mass.  In  low  speed  flight,  the  turn  rates  in  (66)  are  limited  by  the  stall 
angle  of  attack.  The  lift  limit  that  results  from  a  limit  on  angle  of  attack  is  given  by 

L=qSCL=qSCLaa  (67) 

where  the  overbar  indicates  a  limit  value.  Note  that  since  q  is  a  quadratic  function  of  V,  (66) 
implies  that  for  given  alpha,  both  heading  rate  and  flight  path  increase  linearly  with  velocity. 
Thus,  if  the  flight  speed  is  already  being  maintained  as  high  as  possible,  maintaining  a 
commanded  heading  rate  when  on  the  alpha  limit  by  using  throttle  control  is  not  an  option. 

Assuming  that  throttle  is  used  to  maintain  constant  energy,  the  expression  for  the  first  and 
second  time  derivatives  of  altitude  are: 

h  =  V  sin  y 

2 

h  =-g  sin  y  +  V  cos  yy 

The  second  equation  in  (68)  can  be  used  to  solve  for  the  flight  path  angle  rate  needed  to  follow 
the  terrain,  where  y  and  h  are  related  to  the  terrain  data  using 
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siny  =  (fxX  +  fyy)/V 

h  =  fxX  +  f yy  fxx^  ( fxy  fyx  f yy^ 


(69) 


Note  that  the  second  equation  in  (69)  together  with  the  second  equation  in  (68)  establish  an 
algebraic  constraint  on  y  so  long  as  and  fy  do  not  simultaneously  vanish.  Consequently, 

viewing  alpha  as  the  fundamental  limit  results  in  a  limit  on  \j/  through  (66)  that  is  a  complex 
function  of  x,  y  and  y/ .  Therefore,  the  corresponding  costate  equations  become  significantly 
more  complex  when  the  alpha  constraint  becomes  active.  This  is  in  contrast  to  the  situation 
described  so  far  in  this  paper,  where  the  x  and  y  costate  equations  remain  the  same  when  on  the 
heading  rate  limit. 

2.5  Introducing  Velocity  as  a  Control  Variable 

The  constant  energy  formulation  was  originally  introduced  as  a  potentially  more  realistic 
formulation  than  the  constant  velocity  formulation  of  Ref  17,  particularly  for  vehicles  with  low 
thrust  to  weight  ratio.  However,  a  deficiency  of  the  constant  energy  formulation  is  that  it  results 
in  low  velocities  at  high  altitude  and  high  velocities  at  low  altitude,  the  opposite  of  what  might 
be  expected  for  a  realistic  trajectory.  In  a  practical  system,  the  velocity  profile  would  be 
controlled  to  provide  the  highest  possible  airspeed  during  periods  of  straight  flight.  The  question 
becomes  how  velocity  should  be  regulated  during  periods  of  maneuvering  flight.  As  noted 
above,  if  the  aircraft  is  angle  of  attack  limited,  then  the  maximum  turn  rate  increases  linearly 
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with  velocity.  However,  for  terrain  following,  it  can  be  argued  that  turn  radius  is  of  greater 
concern  than  turn  rate.  Since  turn  radius  (R)  is  related  to  turn  rate  (^ )  and  velocity  (10  t>y 

R^  =  V  (70) 

it  follows  that  the  minimum  turn  radius  is  independent  of  velocity.  Thus  is  would  appear  that  the 
constant  velocity  formulation  of  Ref.  14  would  be  appropriate  for  vehicles  whose  turn  radius  is 
angle  of  attack  limited.  However,  vehicles  capable  of  high-speed  flight  are  typically  load  factor 
limited  above  a  speed  known  as  the  comer  velocity.  The  comer  velocity  is  defined  by  the 
condition 


L  =  qSCj^  a  - 


pV^SC,  a  - 
H - ^  =  WG 


(71) 


where  a  is  the  alpha  limit  and  G  is  the  load  factor  limit.  Solving  (71)  for  the  comer  velocity 
we  get 


Vc  = 


2WG 

pSCi^a 


(72) 


The  comer  velocity  can  vary  significantly  depending  on  the  vehicle  characteristics  and  its  limits. 
This  section  considers  a  new  formulation  in  which  both  V  and  tiun  rate  are  treated  as  a 
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control  variable.  To  simplify  the  formulation  of  the  turn  rate  limit,  the  effect  of  gravity  is 
ignored,  and  all  turning  is  assumed  to  take  place  in  the  horizontal  plane.  A  more  detailed 
formulation  is  described  above  in  the  section  Formulating  the  Alpha  Limit,  however  it  is  not 
clear  at  this  stage  if  that  formulation  will  be  tractable.  To  further  simplify  the  presentation,  the 
treatment  is  limited  to  the  simplified  formulation  in  which  the  vertical  component  of  velocity  is 
ignored  in  the  dynamics.  The  dynamics  are: 

x  =  V  cos  y/ 
y  =  V  sin  y/ 
y/  =  u 

The  constraints  are: 

V<V<V 
u\<u(V) 

It  is  assumed  that  the  upper  and  lower  limits  on  V  are  constant.  The  upper  limit  on  u  is  velocity 
dependent,  and  follows  from  the  approximation  (ignoring  gravity) 


(73) 


(74) 


u  =L/  mV 


(75) 


Thus  it  follows  from  (71)  and  (72)  that 
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u(FJ  = 


(76) 


pCi^a 

2m 

gG 

V  ’ 


v<v, 

v>v. 


Note  that  u(V)is  independent  of  V  when  F  <  and  that  du  / dV  is  discontinuous  at  F  =  F^.. 
Clearly,  from  the  earlier  discussion,  this  formulation  is  only  needed  for  the  case  F  >  F^. , 
otherwise  from  the  perspective  of  minimizing  the  time  of  flight,  there  is  no  advantage  to  flying  at 
any  speed  less  that  F  .  We  further  assume  that  F  <  F^ .  Then  it  is  fairly  safe  to  assume  that  the 

lower  limit  on  F  is  never  active,  since  there  is  no  advantage  to  flying  at  a  velocity  less  than  F  . 
With  this  reasoning,  it  also  follows  that  only  the  lower  expression  for  u(V )  applies. 

The  performance  index  is  the  same  as  that  described  earlier 


J=l'^\-Kj  +  Kf(x,y)]dt 


(77) 


where  the  final  time  is  free.  Thus  the  Hamiltonian  for  this  system  satisfies 


H  =  \-K  +  Kf(  x,y )  +  X^V  cosy/  +  XyV  sin  y/  +  Xy^u  +  constra  int  s  =  Q  (78) 


The  constraint  terms  can  be  expressed  as 


9  9  — 

constraints  =  /d\(u  -u  id2(V ~y )  (79) 
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Both  constraint  terms  are  written  in  a  form  such  that  the  quantities  in  parenthesis  are  negative 
when  the  constraints  are  not  active.  Therefore  fii  >  0  when  the  i*'’  constraint  is  active,  and 

fil  =  0  when  i^**  constraint  is  inactive  .  It  is  apparent  from  (78)  that  when  the  constraint  on  u  is 

inactive,  His  a  linear  function  of  both  V  and  u.  Therefore  the  optimal  solution  for  V  is  either  at 
its  limit,  or  is  singular.  Since  final  time  is  a  component  in  the  performance  index,  it  is  probably 
safe  to  rule  out  the  possibility  that  a  singular  arc  in  V  is  optimal.  Therefore,  we  assume  that  when 

the  constraint  on  u  is  inactive,  the  optimal  solution  for  F  is  V*  =  V  .  Furthermore,  when  the 
constraint  on  u  is  inactive  it  follows  that  the  optimal  solution  for  u  is  singular,  in  which  case  we 
have  that  along  a  singular  arc: 


'  (80) 

dr 

The  conditions  in  (80)  can  easily  be  used  to  deduce  the  fact  that  the  trajectory  equations  defining 
an  extremal  arc  can  be  derived  using  the  same  approach  as  above,  with  the  simplification  that  V 
is  constant.  The  same  conclusion  can  be  reached  for  the  tangent  plane  formulation.  Therefore, 
when  the  constraint  on  u  is  inactive,  the  equations  are  simpler  in  form  than  those  above,  and 
reduce  to  the  equations  in  Ref.  14  for  the  tangent  plane  formulation. 

When  the  constraint  on  u  is  active,  then  |m|  =  gG  /  V  and  the  optimal  value  for  V  follows  from 

the  optimality  condition: 
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(81) 


Hw  =  —  =  Xy  COS  +  Av  sin  w  +  U\ - ~ — 

y  QV  ^  ^  y  ^  y3 


=  0 


Now  (81)  applies  only  when  the  resulting  solution  (V*)  satisfies  V  >  ,  since  as  noted  earlier, 

u(V)  has  a  comer  (discontinuous  first  derivative)  at  F  =  .  If  (81)  results  in  a  solution  that  is 

less  than  ,  then  the  minimum  for  H  occurs  at  F  =  F^ .  The  multiplier  //j  can  be  related  to 
using 


H..  =  X. 


+  sgn(M)  =  0 


(82) 


Note  that  since  >  0,  it  follows  firom  (82)  that  sgn(M)  =  -sgn(A^) .  Combining  (81)  and  (82) 
we  have 


F  = 


A_,cos^i^  +  A  sin^</-’ 


F  >K 


(83) 


otherwise  V*  =¥(,.  Combining  (78),  (81)  and  (82)  with  the  fact  that  \u\  =  gG/V  when  the 
constraint  is  active,  results  in 
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H  =  \-K  +  Kf(x.  y)  +  2(X^V  cos  y/  +  AyV  sin  y/)  =  0 


(84) 


Using  the  Hamiltonian  equation  shown  in  equation  (84),  the  following  costate  differential 
equations  can  be  determined 


A.  = 

Ay,  = 


-KL 

-Kfy 

dH 

dy/ 


(85) 


This  leads  to  the  equation  for  Zy, . 

Zy,  =  -IV {Zy  cos{xi/)  -  Z^  sin(^))  (86) 


These  equations  can  then  be  integrated  to  find  the  solution.  As  in  the  heading  rate  constraint 
section,  the  initial  conditions  for  the  costates  are  known  when  the  overall  solution  does  not  start 
on  the  constraint.  Otherwise,  the  initial  conditions  must  be  solved  for. 

In  a  similar  fashion,  the  corresponding  equations  for  the  local  tangent  plane  formulation  can 
be  derived.  For  this  section,  the  state  equations  are 

x  =  M,V 

y  =  -M^V  (87) 

y/  =  u 


where 


M.= 


cos(^y)  ^  /:c/ySin(^y) 


= 


sin(^/) 

^2 


(88) 

(89) 
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and  A2  are  defined  in  equations  (22)  and  (23).  These  equations  lead  to  a  Hamiltonian 
equation  of 

H  =  \-K  +  Kf(x,  y)  +  +  XyVM^  +  constra  int  5  =  0  (90) 

with  the  constraint  term  given  in  equation  (79)  above. 

As  described  previously,  when  the  constraint  on  the  heading  rate  is  not  met,  the  problem 
simplifies  to  the  constant  velocity  formulation  described  in  Ref  14  using  the  maximum  velocity. 
For  the  case  when  the  constraint  is  hit,  the  optimality  conditions  are 

=  =  0  (91) 

»-=4+^^sgn(»)  =  0  (92) 

These  equations  can  then  be  combined  to  find  the  following  optimal  velocity 


V*  <V, 


Combining  (90),  (91)  and  (92)  result  in  the  following  new  Hamiltonian  equation.  • 
H  =  \-K  +  Kf(x,  y)  +  2F(A,Mi  +  )  =  0  (94) 

Equation  (90)  is  used  to  evaluate  the  last  expression  in  (85). 

Xy,=-lV{X,N,-XyN2)  (95) 

with 
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(96) 


-sin(^</')  ^  /c/y 
cos(^«/’) 


(97) 


26 


3.0  Numerical  Results 


3.1  Model  used  for  Generic  Testing 

A  sample  trajectory  was  generated  for  both  the  simplified  and  local  tangent  plane 
formulations  described  above.  In  both  cases,  the  path  was  from  the  initial  point  (-10,  -10)  to  a 
final  point  (10, 10).  The  terrain  was  generated  from  the  function 

z  =  .5  cos(.4a:)  sin(.33^)  + 1  (98) 

and  the  function  f(x,y)  was  chosen  to  equal  Mx,y).  The  trajectories  found  with  each 
formulation  are  essentially  identical.  Figure  1  below  shows  the  path  over  the  terrain  for  K—1 
while  Figure  2  shows  the  trajectory  for  K=0.  In  Figure  2,  this  path  is  basically  a  straight  line,  hi 
Figure  1  it  can  be  seen  that  the  trajectory  winds  arotmd  the  hills. 


Terrain  Elevation 


Terrain  Elevation 


Figure  1:  (-10,  -10)  to  (10,10):  K=1 


Figure  2:  (-10,  -10)  to  (10,10):  K-0 
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Although  the  trajectories  are  nearly  identieal,  this  does  not  imply  that  the  simplified  formulation 
can  be  used  in  place  of  the  tangent  plane  formulation  with  negligible  effect  on  the  guided 
solution  when  the  full  aircraft  dynamics  are  taken  into  account.  This  can  only  be  assessed  using 
a  higher  fidelity  simulation  of  the  aircraft  motion. 

3.2  Real  Terrain  Data 

To  increase  the  realism  of  this  model,  real  terrain  data  was  acquired  from  the  United  States 
Geological  Survey  to  incorporate  into  this  model  [16].  The  data  was  found  in  tabular  format 
relating  the  altitude  to  the  locations  longitude  and  latitude,  with  data  points  spaced  approximately 
every  48  feet.  This  data  was  then  converted  to  matrix  form,  fi'om  which  it  could  then  be  used  as 
/i  y)  •  Because  of  the  distance  between  the  sampled  altitude  points  in  the  matrix,  the  data  was 
then  smoothed  to  appear  more  continuous  and  to  remove  the  discontinuous  leaps  in  altitude.  The 
gradients  of  this  matrix,  along  both  the  x  and  y  directions,  were  calculated  numerically  to  form 
matrices  representing  fi^(x,y)  and  /,^(x,».  The  gradients  of  these  two  matrices  yielded 

matrices  for  f^^{x,y),  and  f^^{x,y). 

For  this  portion  of  the  testing,  it  was  decided  to  use  a  section  of  terrain  near  Columbus,  Ohio. 
A  profile  of  this  terrain  can  be  seen  in  Figure  4.  In  this  graph,  the  x  and  y  axes  depict  the 
position  coordinates,  measured  in  feet,  such  that  the  x  axis  point  north  and  the  y  axis  points  west. 
The  altitude  of  the  terrain  is  measured  along  the  z-axis  and  is  also  given  in  feet.  This  plot 
depicts  a  square  plot  of  land,  with  10,000  feet  to  a  side.  The  measurements  along  the  x  and  y 
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axes  are  relative  to  a  set  origin  for  the  terrain  data  collected;  this  plot  is  just  one  small  piece. 

3.3  Terrain  Path  Results 

Some  initial  results  using  actual  terrain  data  have  been  investigated  using  the  simplified 
formulation  described  above.  The  energy,  which  is  stated  and  held  constant  through  the 
calculations,  was  found  by  determining  the  energy  needed  to  fly  the  vehicle  at  its  minimum  speed 
at  the  highest  altitude  position  in  an  area  near  the  flight  path.  This 


Terrain  Elevation 


Figure  4:  Terrain  Plot  of  an  Area  Near  Columbus  Ohio 

ensures  that  the  necessary  velocity  for  flight  is  never  smaller  than  the  minimum.  However,  if  the 
change  in  altitude  is  too  large,  the  velocity  of  the  plane  can  exceed  its  upper  bounds  at  lower 
altitudes,  so  a  velocity  constraint  will  need  to  be  implemented. 
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Some  flight  paths  have  been  computed  using  the  simplified  formulation  outlined  above. 
Initial  results  from  this  can  be  seen  in  Figures  5  and  6.  In  Figure  5,  it  can  be  seen  that  a  flight 
trajectory  is  outlined  from  the  point  (15500,34000)  to  (18300,31800)  --  this  entails  a  flight  of 
3561  feet.  Figure  6  shows  a  trajectory  of  3688  feet  from  the  point  (18000,  34100)  to  (15200, 
31700).  In  both  plots,  the  black  trajectory  represents  K  =  1  while  the  purple  trajectory  is  for  the 
case  K  =  0. 

TMnin  Etavition  *  jO*  Ttfiain 


Figure  5:  Sample  Flight  Path  from  (15500,34000)  to  (18300,31800)  Using  Simplified 
Formulation.  Black  line  represents  K=1  and  Purple  line  represents  K=0. 


Figure  6:  Sample  Flight  Path  from  (18000,34100)  to  (15200,31700)  Using  Simplified 
Formulation.  Black  line  represents  K=1  and  Purple  line  represents  K=0. 
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Figure  7  depicts  trajectories  created  from  the  local  tangent  plane  formulation.  The  two  plots 
are  for  the  same  initial  and  final  conditions  as  those  shown  in  Figures  5  and  6.  The  results  for  the 
two  formulations  are  very  similar.  Figure  8  shows  a  comparison  of  the  results  of  the  two 
formulations.  In  these  plots,  the  red  line  indicates  the  results  from  the  local  tangent  plane 
formulation  while  the  black  line  represents  the  simplified  form. 


Figure  7:  Sample  Flight  Paths  from  (15500,34000)  to  (18300,31800)  and  (18000,34100)  to 
(15200,31700)  Using  Local  Tangent  Plane  Formulation.  Black  line  represents  K=1 
and  Purple  line  represents  K=0. 


Figure  8:  Sample  Flight  Paths  from  (15500,34000)  to  (18300,31800)  and  (18000,34100)  to 
(15200,31700)  Using  K=l.  Black  line  represents  Simplified  Formulation  and  Red 
line  represents  Local  Tangent  Plane  Formulation. 
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In  addition,  as  can  be  seen  in  the  figures,  the  trajectories  for  both  K  =  0  and  K  =  1  are  similar. 
This  is  because  a  minimum  time  approach  with  constant  energy  will  also  work  to  minimize  the 
altitude  of  flight,  to  some  extent,  due  to  the  fact  that  the  velocity  of  the  vehicle  will  be  greater  at 
lower  altitudes. 


3.4  Modified  Constant  Energy  Approach 

When  using  the  constant  energy  formulation,  the  velocity  changes  greatly  during  a  flight,  as 
has  been  mentioned  earlier.  This  can  lead  to  solutions  that  are  not  flyable.  To  correct  this 
default,  a  modified  constant  energy  formulation  is  implemented  which  utilizes  a  slightly 
modified  definition  of  the  velocity.  The  velocity  definition,  which  can  be  seen  in  equation  (3),  is 
a  direct  function  of  the  position  through  the  terrain  altitude.  The  modified  velocity  is  also  a 
function  of  the  position  through  the  altitude  via  the  equation 

where  Vnom  is  a  given  nominal  velocity  and  f(hhp(x,y))  is  velocity  component  foimd  through  a 
high-pass  filter  of  the  overall  terrain.  With  this  formulation,  the  velocity  will  remain  closer  to  a 
constant  value,  but  will  have  some  fluctuations  due  to  short  term  changes  in  the  altitude. 

The  trajectories  found  using  this  modification  closely  match  the  previous  solutions,  but  with 
a  more  favorable  velocity  profile.  Comparisons  between  the  full  constant  energy  approach  and 
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the  modified  format  can  be  seen  below  in  Figure  9.  This  figure  shows  the  results  using  the 
simplified  equations  of  motion.  The  left  hand  plot  depicts  the  trajectory  found  for  this  situation. 
The  path  is  identical  for  the  two  formulations  used.  The  plot  on  the  right  shows  the  velocity 
versus  time  for  the  two  cases.  The  blue  line  is  the  velocity  for  the  original  constant  energy 
formulation  while  the  green  line  represents  the  velocity  from  the  modified  constant  energy 
formulation.  It  can  be  seen  that  the  green  velocity  varies  much  less  while  the  necessary  time  for 
the  trajectories  is  very  similar. 


Its) 


Figure  9:  Sample  Flight  Path  from  (15500,34000)  to  (18300,31800)  comparing  results  firom 
Original  and  Modified  Constant  Energy  Formulations.  Left  side  shows  path.  Right 
side  shows  velocities  versus  time  with  blue  line  representing  original  form  while 
green  line  represents  modified  form. 

3.5  Implementation  of  a  Heading  Constraint 


A  heading  constraint  has  also  been  successfully  implemented  for  the  constant  velocity 
approach  described  in  Ref.  14.  Results  for  this  can  be  seen  below  in  Figure  10.  This  result  is 
using  the  simplified  formulation  with  the  heading  rate  limited  to  0.02  radians/second  -  the 
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unconstrained  heading  rate  had  a  maximum  of  about  0.035  radians/second.  The  left  side  of  the 
figure  shows  the  plot  of  the  trajectory  found.  The  right  side  shows  a  plot  of  the  heading  rate 
versus  time.  In  this  plot,  it  can  be  seen  that  the  heading  rate  constraint  is  reached  at  the 
beginning  of  the  trajectory  as  well  as  two  other  times  during  the  course  of  the  flight. 
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Figure  10:  Sample  Flight  Path  from  (15500,34000)  to  (18300,31800)  implementing  a  Constant 
Heading  Rate  Constraint  on  the  Simplified  Constant  Velocity  Formulation.  Left  side 
shows  path.  Right  side  shows  heading  rate  versus  time. 


3.6  Flight  Testing  and  Simulator  Implementation 

The  optimal  path  planning  code  using  the  local  tangent  plane  formulation  was  tested  first  oh 
the  Georgia  Tech  helicopter  simulator  and  then  in  actual  flight  tests  with  the  helicopter.  The 
simulator  was  tested  first  to  ensure  the  paths  determined  were  flyable  on  an  actual  aircraft,  before 
the  flight  tests.  At  the  same  time,  the  threat  avoidance  portion  of  the  code  was  tested.  As  can  be 
seen  in  above  in  equations  (3)  and  (4),  there  are  two  different  terms  regarding  terrain-type  inputs. 
One  is  solely  the  terrain  while  the  other  is  a  combined  penalty  function  of  the  terrain  and  threats. 
Previously,  it  was  assumed  that  these  two  functions  were  the  same  —  this  meant  there  were  no 
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threats  other  than  the  terrain.  Ideally,  the  optimal  path  would  avoid  all  the  threats  and  weave 
between  them. 

The  vehicle  used  for  these  tests  was  a  Yamaha  R-Max  helicopter,  seen  in  Figure  11  below. 
This  helicopter  weighs  about  66  kilograms  and  has  a  three-meter  rotor.  It  is  capable  of  flying 
between  approximately  30  —  80  feet  per  second.  In  addition,  it  can  fly  predetermined  paths 
autonomously. 


Figure  11:  Georgia  Institute  of  Technology 
GT-Max  Helicopter 

To  test  the  threat  avoidance  capabilities,  new  terrain  data  files  were  generated  that  used  a 
mostly  flat  terrain  with  threats  imposed  on  it  as  large  hills.  Two  different  methods  of  generating 
these  hills  were  tested.  The  first  was  using  sine  functions  in  the  following  form 

f  =  A  sin(6  *  x)  sin(c  *y)  (1 00) 

Here  A  is  the  amplitude  of  the  hill,  b  and  c  are  scaling  factors  to  adjust  the  width  of  the  hill  and  x 
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and  y  are  the  coordinates  along  the  hill.  Figure  12  shows  one  case  with  this  representation.  It 
was  found  that  this  form  of  creating  the  hills  was  not  successful.  As  can  be  seen  in  equation 
(43),  the  heading  rate  is  dependent  on  the  gradients  of  the  terrain  function.  Therefore,  the  only 
place  where  the  heading  angle  would  change  to  circle  the  threat  is  when  the  path  touches  the  edge 
of  the  threat.  However,  no  matter  what  the  height  or  width  of  the  hill  was,  the  effect  of  touching 
the  edge  of  the  threat  was  to  cause  the  path  to  veer  over  the  top  of  the  hill. 


Terrain  Elevation 


Terrain  Elevation 


Figure  12:  Terrain  with  threats  formulated  as  a  sine  function. 
On  the  right  is  an  overhead  view. 


The  next  method  of  generating  the  terrain  involved  using  an  exponential  function  in  the  form 

/  =  (101) 

where  A  is  the  amplitude,  b  is  a  scaling  factor  to  adjust  the  width  and  r  is  the  distance  from  any 
position  to  the  center  of  the  threat.  A  plot  of  this  representation  can  be  seen  in 
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Terrain  Elevation 


Figure  13:  Terrain  with  threats  formulated  as  an  exponential  function. 

On  the  right  is  an  overhead  view. 

Figure  13.  This  formulation  actually  affects  the  entire  terrain  plot  slightly,  so  the  terrain 
gradients  are  nonzero  for  a  large  portion  of  the  time  as  with  the  sine  function  form. 
Consequently,  the  transition  between  the  flat  terrain  and  the  hills  are  less  abrupt.  This  allows  the 
program  to  find  a  solution  that  avoids  the  threats. 

Three  different  scenarios  were  tested  in  increasing  levels  of  difficulty.  The  first  included  a 
single  threat  in  the  middle  of  the  field,  as  depicted  above  in  Figure  13.  The  second  then  used 
three  threats  while  the  third  included  five  threats.  In  all  cases,  the  threats  were  given  a  diameter 
of  about  300  feet  and  amplitude  of  ten  feet.  This  amplitude  of  ten  feet  was  chosen  because  it  is 
easily  scalable  to  any  desired  amplitude.  The  dimensions  of  the  field  used  were  1000  feet  in  the 
x-direction  pointing  north  and  2000  feet  in  the  y-direction  pointing  east.  In  the  second  case,  the 
three  threats  were  located  at:  (600,1000),  (800,1600),  and  (200,500).  In  the  third  case,  the 
locations  of  the  threats  were:  (500,1000),  (700,1400),  (300,1600),  (750,400),  and  (200,300).  The 
terrain  used  for  the  second  and  third  cases  can  be  seen  below  in  Figure  14. 
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Figure  14:  Terrain  used  in  the  simulations.  The  left  side  shows  the  terrain  with  three  threats. 

The  right  side  shows  the  terrain  with  five  threats. 

For  the  purposes  of  these  simulator  and  flight  tests,  the  local  tangent  plane  formulation  using 
a  constant  velocity  was  used.  This  was  decided  because  the  terrain  was  flat  —  other  than  the 
threats  —  which  would  result  in  the  velocity  from  the  constant  energy  formulation  being  constant. 
A  velocity  of  40  feet  per  second  was  chosen  to  use  with  this  helicopter.  The  results  for  the  three 
tests  can  be  seen  below.  In  each  of  the  plots,  the  black  line  represents  the  trajectory  found  by  the 
program  while  the  blue  line  is  the  actual  path  flown  by  the  helicopter  during  the  flight  tests,  hi 
each  case,  the  starting  position  for  the  path  was  at  the  point  (500,1800)  while  the  final  position 
for  the  path  was  at  (500,200).  In  addition,  the  actual  flight  path,  the  blue  line,  extends  beyond 
these  points.  This  portion  of  the  flight  was  used  to  accelerate  and  decelerate  the  vehicle  so  that 
the  entire  planned  path  between  the  starting  and  ending  points  was  flown  at  a  constant  40  feet  per 
second. 
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Terrain  Elevation  Terrain  Elevation 


Figure  15:  Flight  paths  from  first  case.  The  black  line  is  the 
commanded  path  and  the  blue  line  is  the  actual  path. 


Terrain  Elevation  Terrain  Elevation 


Figure  16:  Flight  paths  from  second  case.  The  black  line  is  the 
commanded  path  and  the  blue  line  is  the  actual  path. 

The  flight  results  for  the  first  case  can  be  seen  in  Figure  15.  This  case  did  not  appear  to 
produce  good  results.  However,  the  problem  was  with  an  axes  error.  The  wrong  file  was 
uploaded  to  the  helicopter  —  one  with  the  axes  rotated.  So,  in  this  case,  the  correct  relative 
trajectory  was  flown,  just  in  the  wrong  direction.  Instead  of  proceeding  west,  the  helicopter 
turned  south  and  flew  the  predetermined  path  in  that  direction;  then  it  continued  to  fly  to  its 
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planned  ending  point.  The  results  to  the  second  and  third  cases  can  be  seen  in  Figures  16  and  17. 
In  both  of  these  cases,  the  actual  flight  results  match  the  optimal  trajectories  very  well. 

Terrain  Etevalion  Terrain  Elevation 


Figure  17:  Flight  paths  from  third  case.  The  black  line  is  the 
commanded  path  and  the  blue  line  is  the  actual  path. 


In  addition,  the  program  was  tested  using  the  simplified  formulation  described  at  the 
beginning  of  the  paper,  with  the  same  three  terrain  maps  described  above.  This  was  done  to 
compare  the  results  from  the  two  different  formulations  for  these  three  terrain  maps.  In  all  of  the 
cases,  the  program  was  unable  to  find  an  optimal  path  that  avoided  the  threats.  As  described 
earlier,  the  optimal  path  should  avoid  going  over  any  of  the  threats.  In  the  first  and  third  cases, 
with  one  and  five  threats,  respectively,  no  solution  at  all  was  found.  In  the  second  case  with 
three  constraints,  the  only  solution  involved  crossing  one  of  the  threats. 
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3.7  Vasa  Control  Variable 


The  code  for  implementing  the  velocity  as  a  control  variable  with  the  local  tangent  plane 
equations  of  motion  was  also  tested.  For  this  section,  the  terrain  data  described  above  for  case  3 
was  used.  In  this  case,  a  velocity  of  80  feet  per  second  is  required  in  order  for  the  heading  rate 
limit  to  be  reached.  Comparatively,  a  velocity  of  about  160  feet  per  second  -  which  is 
unrealistically  high  —  is  necessary  to  reach  the  constraint  for  the  terrain  in  case  2.  The  vehicle 
specifics  used  for  this  case  are  a  weight  of  5  pounds,  a  wing  span  of  2.2  square  feet,  and  a  load 
factor  limit  of  1.1.  The  comer  velocity  for  this  case  is  46.23  feet  per  second  and  the  maximum 
velocity  is  80  feet  per  second. 

Terrain  Elevation  Terrain  Elevation 


Figure  18:  Flight  trajectories  for  the  unconstrained  path  (black  line) 
and  the  constrained  path  (purple  line). 


Figure  18  shows  the  trajectories  from  both  the  unconstrained  code  and  the  velocity  as  a 
control  code.  The  black  line  shows  the  unconstrained  path  while  the  constrained  path  is  depicted 
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in  purple.  It  can  be  seen  in  this  figure  that  the  constrained  path  goes  much  further  around  the 
threat  than  the  unconstrained  trajectory  did.  The  respective  heading  rates  for  these  two  paths  can 
be  seen  in  Figure  19.  The  plot  on  the  left  shows  the  heading  rate  for  the  unconstrained  flight 
while  the  plot  on  the  right  is  the  corresponding  heading  rate  for  the  velocity  as  a  control  path. 
For  the  portion  of  the  constrained  flight  when  the  constraint  is  hit,  the  heading  rate  jumps  to  a 
much  higher  rate.  This  is  due  to  the  fact  that  the  velocity  also  jumps  at  that  point  from  the 
maximum  velocity  to  the  comer  velocity;  this  jumps  causes  the  heading  rate  to  jump  because 
they  are  inversely  proportional,  as  can  be  seen  in  equation  (76).  The  velocity  for  the  constrained 
case  can  be  seen  in  Figure  20. 
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Local  Tangent  Plane  Formulation  ••  psidot  Vasa  Control  -  psidot 


Figure  19:  Heading  rates  for  the  unconstrained  path,  on  the  left, 
and  the  constrained  path,  on  the  right. 


V  as  a  Control  -  V 

90 1 - . - ^ ^ ^ - < - « - 

00 -  -  ' 

70  - 
60  ■ 

50  ■ 

40  ■ 

30  ■ 

20  - 
10  - 

®0  SOO  tOOO  1500  MOO  2S00  3000  3500 


Figure  20:Velocity  profile  for  the  velocity  as  a  control  trajectory 
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3.8  Pop-up  Threats 


The  next  task  investigated  was  the  case  of  pop-up  threats  during  flight.  In  this  case,  the 
optimal  path  is  in  mid-flight  when  a  stationary  threat  appears.  A  new  trajectory  must  then  be 
calculated.  To  test  this,  the  first  results  with  the  Columbus  terrain  -  shown  in  Figure  7  -  were 
used.  However,  the  constant  velocity,  local  tangent  plane  formulation  was  used.  In  this  case,  a 
threat  was  added  to  the  terrain  as  a  single  hill,  as  shown  in  Figure  13.  In  this  case,  the  hill  used 
had  a  height  of  300  feet  above  the  level  of  the  terrain  at  that  point. 
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Figure  21 :  Trajectories  found  with  a  pop-up  threat. 
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The  results  for  this  section  can  be  seen  in  Figure  21.  In  this  plot,  the  black  line  depicts 
the  original  trajectory  found;  here  it  goes  directly  through  the  new  threat.  Three  different  new 
trajectories  are  then  shown  as  magenta  lines.  These  depict  the  results  for  three  different  times  at 
which  the  threat  is  found;  these  times  are  at  17.2  seconds,  29.46  seconds  and  at  39.79  seconds 
into  the  approximately  90  second  flight.  In  each  case,  this  point  is  marked  on  the  plot  with  a  red 
star. 


4.  Summary  of  Phase  I  Program  Results 


The  phase  I  effort  focused  on  demonstrating  a  robust  method  for  numerical  solution  of 
the  reduced  order  problem  that  is  suitable  for  real-time,  on-board  implementation.  The  goal  was 
to  embed  as  much  of  the  full  vehicle  djoiamics  as  possible  in  the  reduced  problem  while 
maintaining  a  tractable  solution  process.  This  was  done  in  steps,  starting  with  the  simplest 
problem  formulation  of  interest,  and  then  adding  complexity  in  small  increments.  The  impact  of 
the  added  complexity  on  efficiency  of  the  numerical  solution  procedure  was  also  evaluated  at 
each  step.  Those  components  that  cannot  be  directly  accounted  for  in  the  reduced  solution  are  to 
be  treated  using  singular  perturbation  methods  of  analysis. 

The  complete  set  of  Phase  I  mathematical  problem  formulations  and  several  solution 
procedures  were  given  in  Chapters  2  and  3.  Representative  numerical  results  were  also 
presented,  as  were  results  fi'om  an  initial  demonstration  of  tracking  obstacle  avoidance 
trajectories  on  a  rotary  wing  UAV.  These  results  were  first  documented  in  the  form  of  a 
technical  paper  presented  by  the  team  members  at  the  AIAA  Guidance,  Navigation  and  Control 
conference  in  August  of  2003.  However,  this  report  includes  many  results  not  available  in  time 
for  conference  paper  submission.  In  this  Chapter,  we  summarize  the  solution  procedures  detailed 
in  Chapters  2  and  3  and  review  their  merits  and  expected  limitations. 

The  effort  began  with  the  formulation  of  the  following  reduced  order  problem;  find  the 
optimal  flight  path  firom  point  a  to  point  b  over  complex  terrain  while  taking  into  account  a 
requirement  to  avoid  static  threats  and  obstacles.  The  work  of  P.K.  Menon,  et.  al.,  in  optimal 
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terrain  following  and  terrain  masking  flight  from  the  early  1990’s  was  found  to  be  directly 
relevant,  and  this  approach  was  adopted  as  the  primary  solution  process  to  be  investigated. 

To  begin,  we  assume  the  availability  of  a  terrain  data  base  for  the  region  in  which  flight 
operations  are  to  be  performed.  We  then  process  the  available  data  to  represent  the  terrain  as  a 
smooth  altitude  profile  as  a  function  of  downrange  and  crossrange  coordinates.  We  next 
introduce  perturbations  in  the  terrain  height  to  represent  threats  or  obstacles  that  are  to  be 
avoided.  That  is,  we  create  fictitious  terrain  features  that  are  representative  of  the  presence  of 
threat  systems,  obstacles,  no  fly  zones,  or  the  like.  Note  that  these  features  can  be  updated  in 
flight  as  new  information  becomes  available  from  on-board  sensors  or  external  sources.  Note 
also  that  the  height  and  shape  of  the  terrain  perturbation  associated  with  a  particular  threat  system 
can  be  adjusted  to  reflect  properties  such  as  its  lethality  and  the  risk  of  exposure  to  it,  as  well  as 
its  relative  importance  in  the  overall  problem  formulation.  Next  the  height  of  the  flight  path 
above  the  terrain  is  constrained  to  remain  fixed  at  a  specified  value  (as  in  nap-of-the-Earth  flight 
operations).  A  simple  performance  index  is  introduced  that  is  the  integral  of  the  sum  of  time  and 
the  modified  terrain  height.  A  weighting  factor  is  used  on  each  component  of  the  integral  to 
allow  the  objective  to  vary  from  the  case  of  absolute  minimum  time,  to  the  case  of  maximum  use 
of  terrain  masking  and/or  threat  avoidance. 

Constant  Energy  Formulation.  We  take  the  indirect  approach  to  set  up  a  solution 
procedure  -  applying  the  1®*  order  necessary  conditions  for  optimality.  In  the  work  of  Menon,  et. 
al.,  flight  is  constrained  to  remain  tangent  to  the  local  terrain  at  constant  velocity.  However,  in 
maneuvering  flight,  it  may  be  undesirable  or  impossible  to  maintain  constant  velocity.  A  better 
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approximation  during  such  maneuvers  may  be  that  of  constant  energy,  allowing  one  to  trade 
potential  and  kinetic  energy  as  needed.  The  team  investigated  two  solutions  to  the  above 
problem  using  the  constant  energy  approximation.  In  the  first,  we  ignore  the  vertical  component 
of  velocity  required  to  follow  the  terrain  height.  The  solution  can  be  reduced  to  the  problem  of 
solving  three  differential  equations  with  only  one  unknown  parameter,  the  initial  aircraft  heading. 
A  simple  numerical  procedure  is  used  to  sweep  through  all  possible  values  of  initial  heading. 
This  results  in  a  very  efficient  solution  procedure  that  is  well  suited  to  real-time  on-board 
implementation.  In  the  second  case,  we  include  the  vertical  component  of  velocity  required  to 
follow  the  terrain  height.  While  the  costate  relations  are  slightly  more  complicated,  in  the  end 
we  obtain  a  solution  of  the  same  form;  integration  of  three  differential  equations  and 
determination  of  the  one  unknown  parameter,  the  initial  vehicle  heading. 

Numerical  results  were  generated  for  both  of  these  cases  using  simple  sinusoidal  and  then 
real  terrain  data.  Sample  results  were  included  in  Chapter  3. 

Additional  of  a  Heading  Rate  Constraint.  Practical  use  of  the  algorithm  will  require 
the  ability  to  enforce  a  heading  rate  constraint.  When  this  constraint  is  not  active  the  solution 
procedure  described  above  remains  unchanged.  When  the  constraint  is  encovmtered  during  flight, 
no  additional  unknowns  result  -  all  of  the  initial  conditions  for  the  constrained  arc  are  equal  to 
their  respective  values  when  the  constraint  is  hit.  In  the  event  the  solution  begins  on  the 
constraint  boundary,  then  the  simple  solution  process  described  above  is  complicated  by  the 
requirement  to  also  determine  the  initial  conditions  on  three  costates. 
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Constraining  Heading  Rate  through  an  Angle  of  Attack  Limit.  As  an  alternative  to 
direct  constraint  of  the  heading  rate,  it  is  possible  to  constrain  it  indirectly  by  imposing  a  limit  on 
angle  of  attack.  The  solution  procedure  was  developed  for  this  case  and  is  summarized  in 
Chapter  2.  However,  the  costate  equations  are  more  complicated  in  this  case,  and  the  direct 
incorporation  of  the  heading  constraint  is  preferred  thus  far. 

Modifled  Constant  Energy  Approach.  In  the  work  described  above,  energy  is  held 
constant  at  a  nominal  value.  This  results  in  the  flight  speed  being  determined  directly  from  the 
terrain  height.  If  we  climb  to  a  high  altitude  in  the  trajectory,  the  use  of  the  energy  state 
approximation  results  in  the  flight  speed  being  determined  by  the  altitude  rather  than  the  cruise 
performance  of  the  aircraft.  Thus  while  the  constant  energy  approximation  is  quite  useful  for 
maneuvering  flight,  it  can  lead  to  inappropriate  choices  of  flight  velocity  during  cruise  unless  the 
nominal  value  of  energy  is  carefully  chosen.  To  overcome  this  limitation  of  the  constant  energy 
formulation,  we  introduce  an  alternate  definition  for  velocity  as  the  linear  combination  of  a 
nominal  value  and  a  perturbation  determined  by  using  the  constant  energy  relation  operating  on  a 
filtered  terrain  data  base.  In  this  formulation  the  nominal  velocity  can  be  set  to  a  standard  flight 
speed,  and  it  will  remain  near  this  value  (i.e.  approximately  constant),  but  will  have  some 
fluctuation  due  to  required  short  term  changes  in  altitude.  The  filter  employed  is  a  high  pass 
filter.  Thus  low  frequency  variation  in  the  terrain  height  is  ignored,  but  steep  altitude  gradients 
are  still  recognized.  The  choice  of  filter  cutoff  frequency  determines  the  blend  that  occurs 
between  the  constant  velocity  solution  and  the  constant  energy  approach.  Note  that  the  velocity 
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calculation  using  the  energy  state  approximation  does  not  use  the  perturbation  in  the  terrain  data 
base  introduced  to  model  threat  systems.  It  only  uses  the  true  terrain  altitude  variation. 
Numerical  results  generated  with  real  terrain  data  were  used  to  compare  the  modified  approach  to 
the  constant  energy  approach.  As  detailed  in  Chapters  2  and  3,  the  resulting  trajectories  and  the 
associated  time  of  flight  are  very  similar,  but  the  variation  in  velocity  is  greatly  reduced.  The 
heading  rate  constraint  described  above  has  also  been  derived  for  the  constant  velocity  case.  This 
approach  was  found  to  provide  the  benefits  of  the  constant  energy  approach  (i.e.  it  recognizes  the 
need  to  trade  kinetic  and  potential  energy  when  the  vehicle  must  climb  rapidly  to  avoid  terrain, 
but  does  not  force  the  flight  velocity  to  an  inappropriate  value  when  climbing  or  descending  in 
general).  Other  than  introduction  of  the  terrain  height  filter  and  the  new  calculation  of  velocity, 
this  approach  enjoys  the  same  solution  procedure  as  described  above  (simple  sweep  of  initial 
heading  value). 

Employing  Velocity  as  a  Control  Variable.  In  all  of  the  work  described  above,  velocity 
is  just  a  consequence  of  the  other  states.  We  next  considered  the  use  of  velocity  as  a  control 
variable,  as  detailed  in  later  chapters.  The  optimal  solution  in  this  case  results  in  a  simple 
strategy  of  flying  at  the  velocity  limit  except  when  limited  in  a  maneuver  by  the  g-limit.  In  such 
case  the  velocity  is  limited  to  respect  the  g-limit.  The  relations  that  must  be  solved  in  this  case 
are  more  complicated,  but  still  tractable.  And  as  in  the  case  when  the  angle  of  attack  limit  is 
introduced,  if  the  solution  starts  on  the  g-limit,  one  must  also  solve  for  the  initial  conditions  on 
the  3  costates.  Since  velocity  is  treated  as  a  control  variable  in  the  reduced  solution,  it  is  possible 


50 


that  velocity  can  be  discontinuous.  In  such  case  a  boundary  layer  correction  is  required. 
However,  it  is  expected  that  this  will  rarely  be  the  case  when  working  with  real  terrain  data. 


5.  Assessment  of  Feasibility 

The  phase  I  program  was  designed  to  demonstrate  the  feasibility  of  developing  algorithms 
appropriate  for  real-time  autonomous  air  vehicle  trajectory  optimization  using  the  method  of 
singular  perturbations  to  exploit  natural  time-scale  separation  in  the  vehicle  dynamics.  In  such 
case,  the  reduced  order  problem  must  only  address  the  translational  dynamics,  and  a  boundary 
layer  analysis  is  used  to  accoimt  for  neglected  nonlinear  aircraft  dynamics  and  to  produce  an 
optimal  feedback  guidance  solution.  The  focus  of  the  phase  I  effort  was  development  of  robust 
numerical  solution  procedures  for  the  reduced  order  problem  in  the  presence  of  complex  3-D 
terrain,  threats  and  obstacles. 

Excellent  results  were  obtained  from  the  phase  I  effort.  As  summarized  in  the  previous 
Chapter,  variations  of  the  reduced  order  problem  have  been  formulated  that  embed  ever  greater 
amounts  of  the  full  problem  complexity,  and  in  each  case  the  solution  process  was  reduced  by 
application  of  the  necessary  conditions  to  a  simple  1  parameter  numerical  search.  The  only 
exception  is  when  the  solution  starts  on  a  constraint  boundary.  In  this  special  case  the  search 
must  be  expanded  to  include  the  costate  initial  conditions.  Constraints  on  turn  rate,  angle  of 
attack,  and  g-limits  were  successfully  introduced.  C  code  versions  of  the  algorithms  were 
developed  and  tested  using  realistic  terrain  data,  demonstrating  practical  solution  over  complex 
terrain  in  seconds  on  a  standard  desktop  PC. 
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The  phase  I  results  clearly  establish  the  feasibility  of  producing  robust  numerical  solution 
to  the  reduced  order  problem  despite  the  complications  of  operation  in  terrain  amongst  obstacles 
and  threats.  The  resulting  one-parameter  search  problem  can  be  solved  very  rapidly  with  a 
simple  numerical  sweep  and  will  support  real-time  autonomous  execution  of  the  optimization 
algorithm  on-board.  The  simple  phase  I  process  by  which  the  step  size  is  adjusted  in  this  one- 
parameter  search  is  adequate,  but  can  be  improved  in  phase  n.  It  is  also  possible  that  one  can 
satisfy  all  of  the  necessary  conditions  to  arrive  at  a  solution,  but  still  not  have  the  true  optimal 
solution.  Application  of  the  2"^  order  necessary  conditions  may  provide  additional  means  to 
identify  the  optimal  solution  in  this  case,  and  is  to  be  the  subject  of  investigation  in  phase  H. 

The  envisioned  phase  n  program  will  continue  the  work  of  phase  I  to  fully  develop 
efficient  numerical  solution  procedures  for  complex  variations  of  the  reduced  order  problem.  In 
particular  the  reduced  problem  formulation  should  be  expanded  to  include  time  varying  threats, 
moving  terminal  conditions,  and  interior  point  constraints.  Numerical  solutions  of  the  full-order 
optimization  problem  for  candidate  mission  scenarios  could  be  developed  to  provide  a  basis  for 
evaluating  the  reduction  in  optimality  experienced  when  employing  the  reduced  solution.  This 
“truth”  model  could  also  be  used  to  identify  those  maneuvers  where  bovmdary  layer  corrections 
are  needed.  The  boundary  layer  corrections  can  then  be  developed,  as  well  as  associated  optimal 
feedback  guidance  laws.  As  noted  above,  the  2"^  order  necessary  conditions  should  also  be 
explored. 
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6.  Recommendations  for  Further  Research 

The  specific  objectives  proposed  for  a  phase  n  program  are  presented  below. 

•  Develop  a  comprehensive  set  of  Phase  n  mission  scenarios  that  is  representative  of  the 
sponsor’s  program  interests. 

•  Extend  the  Phase  I  reduced  order  problem  formulation  and  numerical  solution  procedure 
to  include: 

-  Moving  threat  systems 

-  Moving  terminal  conditions 

-  Interior  point  constraints 

-  Three-dimensional  maneuvers  with  velocity  as  a  control 

•  Evaluate  the  ability  to  track  the  developed  reduced  order  solution  optimal  trajectories  in 
high-fidelity  nonlinear  simulation  of  six  degree-of-fi‘eedom  high-performance  aircraft 
operating  at  low  altitude  over  real  terrain  with  realistic  simulation  of  both  stationary  and 
moving  pop-up  threats. 

•  For  a  select  number  of  candidate  trajectories,  construct  the  full-order  optimal  trajectory 
using  a  multiple  shooting  algorithm  such  as  BOUNDSCO. 
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•  Compare  the  full-order  and  reduced-order  optimal  trajectories,  and  quantify  the 
differences.  Fly  both  trajectories  with  a  nominal  guidance  law  in  the  simulation  and 
again  quantify  the  differences.  Identify  any  flight  maneuvers  for  which  construction  of  a 
boundary  layer  solution  is  expected  to  be  beneficial. 

•  Perform  boundary  layer  construction  for  the  selected  flight  phases,  if  any,  where  a 
significant  improvement  in  optimality  has  been  shown  to  be  available. 

•  Repeat  flight  simulations  of  the  candidate  trajectories  using  the  boundary  layer 
corrections,  compare  with  the  full-order  optimal  solution,  and  quantify  the  improvements 
obtained. 

•  Further  investigate  the  possibility  of  conjugate  points,  and  develop  a  test  for  the 
occurrence  of  these  points . 

•  Evaluate  and  demonstrate  the  real-time  performance  of  the  developed  optimization 
algorithm  in  high-fidelity  nonlinear  simulation  for  the  full  range  of  mission  scenarios 
being  investigated. 

•  Flight  demonstrate  guided  flight  of  a  UAV  while  employing  the  real-time  path 
optimization  algorithm  in  at  least  three  challenging  mission  scenarios  by  introducing 
virtual  terrain,  obstacles  and  pop-up  threats. 
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